#include "sift_api.h"


#define SOURCE_MARKER_SIZE 100
#define SOURCE_IMAGE_SIZE  1000

using namespace std;
using namespace cv;

struct stRectParam
{
	int		m_minSideLength; // 30
	int		m_maxSideLength;	// 1000   radius
	double	m_minArea;
	double	m_maxArea;

	int m_approxPolyDPSideNumber;
	double m_ratio;   // LongEdge / shortEdge 
	double m_includeAngle;

	int  m_externalGoodMatchesMinNum;
	int  m_internalGoodMatchesMinNum;
};


static double wrap360(const double angle)
{
    double res = fmod(angle, 360.0);
    if (res < 0)
    {
        res += 360.0;
    }
    return res;
}


bool siftHandleImage(const Mat &imGray, vector<KeyPoint> &kp, Mat &des)
{
	if(imGray.empty())
	{
		cout << "image empty" << endl;
		return false;
	}

	int zoomScaleWidth;
	int zoomScaleHeight;

	zoomScaleWidth = SOURCE_MARKER_SIZE;
	zoomScaleHeight = zoomScaleWidth * imGray.rows / imGray.cols;

	Mat tmp;
	resize(imGray, tmp, Size(zoomScaleWidth, zoomScaleHeight),INTER_AREA);

	Ptr<Feature2D> sift = xfeatures2d::SIFT::create();

	sift->detect(tmp, kp);
	sift->compute(tmp, kp, des);

	return true;
}



bool computeH(vector<DMatch> &matches, vector<KeyPoint> &soureKp,
						vector<KeyPoint> &targetKp, int &goodMatchesNum,double &rationAngle, int &returnGoodMatchesNum)
{
	if(matches.size() <= goodMatchesNum)
	{
		return false;
	}

	double minDist = matches[0].distance, maxDist = matches[0].distance;

	for(int i=0; i<matches.size(); i++)
	{
		if(matches[i].distance < minDist)
		{
			minDist = matches[i].distance;
		}

		if(matches[i].distance > maxDist)
		{
			maxDist = matches[i].distance;
		}
	}

	vector<DMatch> goodMatches;
	for(int i=0; i < matches.size(); i++)
	{
		if(matches[i].distance < 0.6 * maxDist)
		{
			goodMatches.push_back(matches[i]);
		}
	}

	if(goodMatches.size() < goodMatchesNum)
	{
		return false;
	}

	vector<KeyPoint> sourceKeyPoint, targetKeyPoint;
	for(size_t i=0; i<goodMatches.size(); i++)
	{
		sourceKeyPoint.push_back(soureKp[goodMatches[i].queryIdx]);
		targetKeyPoint.push_back(targetKp[goodMatches[i].trainIdx]);
	}

	vector<Point2f>  sroucePoint, targtePoint;
	for(size_t i=0; i<goodMatches.size(); i++)
	{
		sroucePoint.push_back(sourceKeyPoint[i].pt);
		targtePoint.push_back(targetKeyPoint[i].pt);
	}


	//compute homography
	vector<char> inLiners;
	Mat homography = findHomography(sroucePoint, targtePoint,inLiners, RANSAC, 1.0);

	if(homography.empty())
	{
		return false;
	}


	//scale factor x
	double a = homography.at<double>(0, 0);
	//cut factor xy
	double b = homography.at<double>(0, 1);
	//translation x
	double c = homography.at<double>(0, 2);
	//cut factor yx
	double d = homography.at<double>(1, 0);
	//scale factor y
	double e = homography.at<double>(1, 1);
	//translation y
	double f = homography.at<double>(1, 2);

	//set ration angle
	rationAngle = atan2(d, a) * 180 / CV_PI;
	//rationAngle = wrap360(rationAngle);

	//extract the number of inliners
	int inLinersCount = 0;
	for (size_t i=0; i<goodMatches.size(); i++)
	{
		if(inLiners[i] !=0)
		{
			inLinersCount++;
		}
	}

	//cout << "++++goodMatch:" << goodMatches.size() << ",ran match:" << inLinersCount << endl;

	if(inLinersCount < goodMatchesNum)
	{
		return false;
	}

	returnGoodMatchesNum = inLinersCount;

	return true;
}



bool computeLandPosition(const Mat &imGray, vector<KeyPoint> &markerExKp, Mat &markerExDes,
						vector<KeyPoint> &markerInKp, Mat &markerInDes, vector<Point> &rect, double &rationAngle,
						int &inOrEx, int &externalGoodMatchesMinNum, int &internalGoodMatchesMinNum, int &returnGoodMatchesNum, bool camerIsFlat)
{
	if(imGray.empty())
	{
		cout << "image empty" << endl;
		return false;
	}

	struct stRectParam rectParam;
	rectParam.m_minSideLength = 30;
	rectParam.m_maxSideLength = 1000;
	rectParam.m_minArea = pow(rectParam.m_minSideLength, 2);
	rectParam.m_maxArea = pow(rectParam.m_maxSideLength, 2);
	rectParam.m_approxPolyDPSideNumber = 4;

	// LongEdge 0.85m , shortEdge 0.66 m
	rectParam.m_ratio = 0.85f / 0.66f;
	rectParam.m_includeAngle = 90;

	rectParam.m_externalGoodMatchesMinNum = externalGoodMatchesMinNum;
	rectParam.m_internalGoodMatchesMinNum = internalGoodMatchesMinNum;

	Mat shrinkImGray;
	float shrinkRatio;
	shrinkRatio = (float)imGray.cols / (float)SOURCE_IMAGE_SIZE;
	resize(imGray, shrinkImGray, Size(imGray.cols / shrinkRatio, imGray.rows / shrinkRatio),INTER_AREA);

	//gaussian
	Mat gaussianBlurOutput;
	GaussianBlur(shrinkImGray,gaussianBlurOutput,Size(5,5), 1, 1);

	// canny
	Mat cannyOutput;
	Canny( gaussianBlurOutput, cannyOutput, 100, 100 * 2, 3 );

	//findContours
	vector<vector<Point>> contours;
	vector<Vec4i> hierarchy;

	findContours(cannyOutput, contours, hierarchy, RETR_CCOMP, CHAIN_APPROX_SIMPLE);

	for (int i = 0; i < contours.size(); i++)
	{
		double area = contourArea(contours[i]);

		if(area < rectParam.m_minArea / shrinkRatio || area > rectParam.m_maxArea / shrinkRatio)
		{
			continue;
		}

		vector<Point> lines;
		approxPolyDP(contours[i],lines,10,true);

		if(lines.size()  != rectParam.m_approxPolyDPSideNumber)
		{
			continue;
		}

		float widthEdge, heightEdge;
		widthEdge = sqrt(pow(lines[1].x - lines[0].x, 2) + pow(lines[1].y - lines[0].y, 2));
		heightEdge = sqrt(pow(lines[2].x - lines[1].x, 2) + pow(lines[2].y - lines[1].y, 2));

		float longShortRatio =  widthEdge > heightEdge ? widthEdge / heightEdge : heightEdge / widthEdge;

		//check long / short ratio
		if(longShortRatio > rectParam.m_ratio * 1.15f && camerIsFlat)
		{
			continue;
		}

		//check the anlge
		float cosDalt = (float)((lines[0].x - lines[1].x) * (lines[2].x - lines[1].x) + (lines[0].y - lines[1].y) * (lines[2].y - lines[1].y)) / (widthEdge * heightEdge);

		double dalt = acos(cosDalt) * (180.0f / CV_PI);
		dalt = wrap360(dalt);

		if((dalt < rectParam.m_includeAngle * 0.95f || dalt > rectParam.m_includeAngle * 1.05f) && camerIsFlat)
		{
			continue;
		}

		//set the rect four corne point
		rect.clear();
		for(int j=0; j<lines.size(); j++)
		{
			Point rectPoint;
			rectPoint.x = lines[j].x * shrinkRatio;
			rectPoint.y = lines[j].y * shrinkRatio;
			rect.push_back(rectPoint);
		}

		//compute the rect scals 
		int startX, startY, endX, endY;
		startX = std::min({lines[0].x, lines[1].x, lines[2].x, lines[3].x});
		startY = std::min({lines[0].y, lines[1].y, lines[2].y, lines[3].y});
		endX = std::max({lines[0].x, lines[1].x, lines[2].x, lines[3].x});
		endY = std::max({lines[0].y, lines[1].y, lines[2].y, lines[3].y});

		int width, height;
		width = endX - startX;
		height = endY - startY;

		Point center;
		center.x = ((startX + endX) / 2) * shrinkRatio;
		center.y = ((startY + endY) / 2) * shrinkRatio;

		//save center point
		rect.push_back(center);


		//cut the rect image to a new image
		Mat temp = shrinkImGray.clone();

		for(int row=startY; row <= endY; row++)
		{
			for(int col = startX; col <=endX; col++)
			{
				Point2f p(col, row);
				double ret = pointPolygonTest(lines,  p, false);
				if(ret < 0)//outer point
				{
					//255 is white
					temp.at<uchar>(row,col) = 255;
				}
			}
		}

		Rect rectTmp = Rect( startX, startY, width, height);
		Mat extractIm = (Mat( temp, rectTmp)).clone();

		float zoomRatio;
		zoomRatio = (float)extractIm.cols / (float)SOURCE_MARKER_SIZE;

		resize(extractIm, extractIm, Size(extractIm.cols / zoomRatio, extractIm.rows / zoomRatio),INTER_AREA);

		//get sifi keypoint and descriptor
		vector<KeyPoint> extractKp;
		Mat extractDes;

		bool ret = siftHandleImage(extractIm, extractKp, extractDes);

		if(ret == false)
		{
			//will not go here.
			cout<< "extractim sift handle image is error!" << endl;
			continue;
		}

		//according to descopter do match
		Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce");
		std::vector<DMatch> matches;

		switch(inOrEx)
		{
			case AllQrCode:
			{
				//match external marker
				matches.clear();
				matcher->match(markerExDes, extractDes, matches);

				ret = computeH(matches, markerExKp, extractKp, rectParam.m_externalGoodMatchesMinNum, rationAngle, returnGoodMatchesNum);

				if(ret)
				{
					inOrEx = ExternalQrCode;
					//get the external marker and land position, so return
					return true;
				}

				//match internal marker
				matches.clear();
				matcher->match(markerInDes, extractDes, matches);

				ret = computeH(matches, markerInKp, extractKp, rectParam.m_internalGoodMatchesMinNum, rationAngle, returnGoodMatchesNum);

				if(ret)
				{
					inOrEx = InternalQrCode;
					//get the internal marker and land position, so return
					return true;
				}
				break;
			}
			case ExternalQrCode:
			{
				//match external marker
				matches.clear();
				matcher->match(markerExDes, extractDes, matches);

				ret = computeH(matches, markerExKp, extractKp, rectParam.m_externalGoodMatchesMinNum, rationAngle, returnGoodMatchesNum);

				if(ret)
				{
					inOrEx = ExternalQrCode;
					//get the external marker and land position, so return
					return true;
				}
				break;
			}
			case InternalQrCode:
			{
				//match internal marker
				matches.clear();
				matcher->match(markerInDes, extractDes, matches);

				ret = computeH(matches, markerInKp, extractKp, rectParam.m_internalGoodMatchesMinNum, rationAngle, returnGoodMatchesNum);

				if(ret)
				{
					inOrEx = InternalQrCode;
					//get the internal marker and land position, so return
					return true;
				}
				break;
			}
		}
	}

	inOrEx = AllQrCode;
	return false;
}
